////
//// Created by ze on 2023/12/30.
////
//
#include "UPRE_ROBOT_TASK.h"
#include "UPRE_PATH.h"
#include "UPRE_PID.h"
#include "UPRE_MOVEBASE.h"
#include "UPRE_LOCATION.h"
#include "cmsis_os.h"
////#include "UPRE_Hand_Shank.h"
////#include "UPRE_AirCylinder.h"
#include "UPRE_DIJKSTRA.h"
#include "UPRE_GYRO.h"
#include "UPRE_Task_Flow.h"
////#include "UPRE_Decision.h"
////#include "UPRE_DIJKSTRA.h"
//// Created by ze on 2023/11/10.
////
////
//uint8_t chassis[26] = {0};
uint8_t g_Auto_Num = 0; ////48 //////55
//static uint8_t Only_Once = 0;
////直线路径任务
//static StraightLinePath line = {0};
//
bool Flag_Finish1Task = false;
bool Flag_ManualStart = false;
//
void DemarcatedPath(void) {
    static StraightLinePath Control_Point;
    switch (g_Auto_Num) {
        case 0:
            Control_Point.startPose = Robot_Pose;
        //标定X轴
        //            Control_Point.endPose.wx = 250000.f;
        //            Control_Point.endPose.wy = 0.f;
        //标定Y轴
            Control_Point.endPose.wx = 0.f;
            Control_Point.endPose.wy = 250000.f;

            Control_Point.endPose.yaw = 0.F;

            g_Auto_Num++;

            break;
        case 1:
            if (StraigthLinePathInit(&Control_Point, false, 5000, 5000, 0, 5, 5, 10)) {
                g_Auto_Num++;
            }
            break;
        case 2:
            if (goPath() == FINISH) {
                //                setPidCalMode(PID_OFF);
                g_Auto_Num++;
            }
            break;
        /////路径一结束
        default:
            break;
    }
}


//void GoBesselTask1(void) {
//    static PATH_BesselControlPoint Control_Point;
//    static Pose gopath3 = {-2565.53f, 2301.34f, -89.73f}; //直线路径3
//    static Pose gopath6 = {(-2098.6f), (2049.3f), -270.f}; //直线路径6
//    static Pose gopath9 = {(-1141.25f), (2263.85f), -449.6f}; //直线路径9
//    static Pose gopath12 = {(-683.26f), (1983.39f), -630.f}; //直线路径12
//    static Pose gopath13 = {(642.f), (2044.14f), 90.f}; //直线路径13     //平移到坡前
//    static Pose gopath14 = {(725.78f), (5554.29f), 90.f}; //开始上坡
//    switch (g_Auto_Num) {
//        /////路径一开始          //第一次取苗
//        case 0:
//            if (/*Pack_receive.is_start_chassic*/1) {
//                Control_Point.Path_BesselPoint_1 = Robot_Pose;
//
//                Control_Point.Path_BesselPoint_4.wx = 0.f;
//                Control_Point.Path_BesselPoint_4.wy = 0.f;
//                Control_Point.Path_BesselPoint_4.yaw = 0.f;
//
//                Control_Point.Path_BesselPoint_2.x =
//                        (Control_Point.Path_BesselPoint_4.wx - Robot_Pose.wx) * 0.3f + Robot_Pose.wx;
//                Control_Point.Path_BesselPoint_2.y =
//                        (Control_Point.Path_BesselPoint_4.wy - Robot_Pose.wy) * 0.3f + Robot_Pose.wy;
//
//                Control_Point.Path_BesselPoint_3.x =
//                        (Control_Point.Path_BesselPoint_4.wx - Robot_Pose.wx) * 0.6f + Robot_Pose.wx;
//                Control_Point.Path_BesselPoint_3.y =
//                        (Control_Point.Path_BesselPoint_4.wx - Robot_Pose.wx) * 0.6f + Robot_Pose.wx;
//
//                g_Auto_Num++;
//            }
//            break;
//        case 1:
//
//            if (Path_BesselPathInit(&Control_Point, false, 3500, 3500, 100, 5, 40, 80));
//            chassis[10] = 1;
//            g_Auto_Num++;
//
//            break;
//        case 2:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//
//                g_Auto_Num++;
//            }
//            break;
//        /////路径一结束
//        case 3:
//
//            /****右取苗开始****/
//
//            chassis[0] = 1;
//
//        /****右取苗结束****/
//        //            g_Auto_Num++;
//        //            Motor_ClearPID();
//            break;
//        /////路径二开始                                  //第一次放苗
//        case 4:
//
//            Control_Point.Path_BesselPoint_1 = Robot_Pose;
//            Control_Point.Path_BesselPoint_2.x = -3212.f; //-3355+143
//            Control_Point.Path_BesselPoint_2.y = 1017.f; //190+827
//
//            Control_Point.Path_BesselPoint_3.x = -3183.f; //-3061-122
//            Control_Point.Path_BesselPoint_3.y = 1566.f; //2070.22f -504.22
//
//            Control_Point.Path_BesselPoint_4.wx = -3026.33f;
//            Control_Point.Path_BesselPoint_4.wy = 2085.22f;
//            Control_Point.Path_BesselPoint_4.yaw = -90.f;
//            g_Auto_Num++;
//
//            break;
//        case 5:
//            if (Path_BesselPathInit(&Control_Point, false, 2000, 2500, 100, 5, 40, 70));
//
//            g_Auto_Num++;
//
//            break;
//        case 6:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//
//                g_Auto_Num++;
//            }
//            break;
//        /////路径二结束
//        case 7:
//            //            HAL_GPIO_TogglePin(LED2_GPIO_Port,LED2_Pin);
//            /****右放苗2开始****/
//
//            chassis[2] = 1;
//
//        /****右取苗2结束****/
//        //            g_Auto_Num++;
//            break;
//        /////路径三开始                         //第二次放苗
//        case 8:
//            line.startPose = Robot_Pose;
//            line.endPose = gopath3;
//            line.endPose.yaw = optimizeRobRotation(line.endPose.yaw);
//
//            g_Auto_Num++;
//
//            break;
//        case 9:
//            if (StraigthLinePathInit(&line, false, 1500, 1500, 100, 10, 60, 20));
//
//            g_Auto_Num++;
//
//            break;
//        case 10:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//                g_Auto_Num++;
//            }
//            break;
//        /////路径三结束
//        case 11:
//            /****右放苗1开始****/
//
//            chassis[3] = 1;
//
//        /****右放苗1结束****/
//        //            g_Auto_Num++;
//        //            Motor_ClearPID();
//            break;
//        /////路径四开始              //第二次取苗
//        case 12:
//
//            Control_Point.Path_BesselPoint_1 = Robot_Pose;
//
//            Control_Point.Path_BesselPoint_2.x = -2021.93f;
//            Control_Point.Path_BesselPoint_2.y = 2209.04f;
//
//            Control_Point.Path_BesselPoint_3.x = -2770.18f;
//            Control_Point.Path_BesselPoint_3.y = 887.1f;
//
//            Control_Point.Path_BesselPoint_4.wx = -2652.f;
//            Control_Point.Path_BesselPoint_4.wy = 140.1f;
//            Control_Point.Path_BesselPoint_4.yaw = -180.25f;
//
//            g_Auto_Num++;
//
//            break;
//        case 13:
//            if (Path_BesselPathInit(&Control_Point, false, 3000, 3000, 100, 5, 50, 60));
//
//            g_Auto_Num++;
//
//            break;
//        case 14:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//                g_Auto_Num++;
//            }
//            break;
//        /////路径四结束
//        case 15:
//            /****左取苗开始****/
//
//            chassis[1] = 1;
//
//        /****左取苗结束****/
//        //            g_Auto_Num++;
//            break;
//        /////路径五开始                  //第三次放苗
//        case 16:
//            Control_Point.Path_BesselPoint_1 = Robot_Pose;
//
//            Control_Point.Path_BesselPoint_2.x = -2534.f;
//            Control_Point.Path_BesselPoint_2.y = 1005.1f;
//
//            Control_Point.Path_BesselPoint_3.x = -2710.1f;
//            Control_Point.Path_BesselPoint_3.y = 1352.88f;
//
//            Control_Point.Path_BesselPoint_4.wx = -2563.1f;
//            Control_Point.Path_BesselPoint_4.wy = 1852.1f;
//            Control_Point.Path_BesselPoint_4.yaw = -268.f;
//
//            g_Auto_Num++;
//
//            break;
//        case 17:
//            if (Path_BesselPathInit(&Control_Point, false, 2000, 2500, 100, 5, 40, 70));
//            g_Auto_Num++;
//
//            break;
//        case 18:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//                g_Auto_Num++;
//            }
//            break;
//        /////路径五结束
//        case 19:
//            /****左放苗1开始****/
//
//            chassis[4] = 1;
//
//        /****左放苗1结束****/
//        //            g_Auto_Num++;
//        //            Motor_ClearPID();
//            break;
//        /////路径六开始                  //第四次放苗
//        case 20:
//
//            line.startPose = Robot_Pose;
//            line.endPose = gopath6;
//            line.endPose.yaw = optimizeRobRotation(line.endPose.yaw);
//            g_Auto_Num++;
//
//            break;
//        case 21:
//            if (StraigthLinePathInit(&line, false, 1000, 1000, 100, 0, 25, 20));
//            g_Auto_Num++;
//            break;
//        case 22:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//                g_Auto_Num++;
//            }
//            break;
//        /////路径六结束
//        case 23:
//            /****左放苗2开始****/
//
//            chassis[5] = 1;
//
//        /****左放苗2结束****/
//        //            g_Auto_Num++;
//        //            Motor_ClearPID();
//            break;
//        /////路径七开始              //第三次取苗
//        case 24:
//            Control_Point.Path_BesselPoint_1 = Robot_Pose;
//        //            static Pose gopath6 = { (-2118.6f), (2064.3f),-270.8f};  //直线路径6
//
//            Control_Point.Path_BesselPoint_2.x = -1565.f; //-2118.6+553.6
//            Control_Point.Path_BesselPoint_2.y = 1957.f; //2064.3-107.3
//
//            Control_Point.Path_BesselPoint_3.x = -2090.f; //-1996.82-93.18
//            Control_Point.Path_BesselPoint_3.y = 889.f; //180.f+709
//
//            Control_Point.Path_BesselPoint_4.wx = -1971.82f;
//            Control_Point.Path_BesselPoint_4.wy = 155.f;
//            Control_Point.Path_BesselPoint_4.yaw = -360.f; //TODO
//
//            g_Auto_Num++;
//            break;
//        case 25:
//
//            if (Path_BesselPathInit(&Control_Point, false, 3000, 3000, 100, 5, 40, 60));
//
//            g_Auto_Num++;
//
//            break;
//        case 26:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//                g_Auto_Num++;
//            }
//            break;
//        /////路径七结束
//        case 27:
//
//            /****右取苗开始****/
//
//            chassis[0] = 1;
//
//        /****右取苗结束****/
//        //            g_Auto_Num++;
//        //            Motor_ClearPID();
//            break;
//        /////路径八开始      //第五次放苗
//        case 28:
//
//            Control_Point.Path_BesselPoint_1 = Robot_Pose;
//
//            Control_Point.Path_BesselPoint_2.x = -1853.82f;
//            Control_Point.Path_BesselPoint_2.y = 1007.f;
//
//            Control_Point.Path_BesselPoint_3.x = -1767.83f;
//            Control_Point.Path_BesselPoint_3.y = 1545.18f;
//
//            Control_Point.Path_BesselPoint_4.wx = -1610.83f;
//            Control_Point.Path_BesselPoint_4.wy = 2034.4f;
//            Control_Point.Path_BesselPoint_4.yaw = -452.f;
//
//            g_Auto_Num++;
//
//            break;
//        case 29:
//            if (Path_BesselPathInit(&Control_Point, false, 2000, 2500, 100, 5, 40, 70));
//
//            g_Auto_Num++;
//
//            break;
//        case 30:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//
//                g_Auto_Num++;
//            }
//            break;
//        /////路径八结束
//        case 31:
//            /****右放苗2开始****/
//
//            chassis[2] = 1;
//
//        /****右取苗2结束****/
//        //            g_Auto_Num++;
//        //            Motor_ClearPID();
//            break;
//        /////路径九开始                  // 第六次放苗
//        case 32:
//            line.startPose = Robot_Pose;
//            line.endPose = gopath9;
//            line.endPose.yaw = optimizeRobRotation(line.endPose.yaw);
//
//            g_Auto_Num++;
//
//            break;
//        case 33:
//            if (StraigthLinePathInit(&line, false, 1200, 1200, 0, 0, 30, 20));
//
//            g_Auto_Num++;
//
//            break;
//        case 34:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//                g_Auto_Num++;
//            }
//            break;
//        /////路径九结束
//        case 35:
//            /****右放苗1开始****/
//
//            chassis[3] = 1;
//
//        /****右放苗1结束****/
//        //            Motor_ClearPID();
//        //            g_Auto_Num++;
//            break;
//        /////路径十开始          //第四次取苗
//        case 36:
//            Control_Point.Path_BesselPoint_1 = Robot_Pose;
//        //            static Pose gopath9 = { (-1176.25f), (2278.85f),-449.6f};  //直线路径9
//
//            Control_Point.Path_BesselPoint_2.x = -622.65f;
//            Control_Point.Path_BesselPoint_2.y = 2171.55f;
//
//            Control_Point.Path_BesselPoint_3.x = -1398.04f;
//            Control_Point.Path_BesselPoint_3.y = 851.f;
//
//            Control_Point.Path_BesselPoint_4.wx = -1274.86f;
//            Control_Point.Path_BesselPoint_4.wy = 120.f;
//            Control_Point.Path_BesselPoint_4.yaw = -540.f;
//
//            g_Auto_Num++;
//
//            break;
//        case 37:
//            if (Path_BesselPathInit(&Control_Point, false, 3000, 3000, 100, 5, 40, 60));
//
//            g_Auto_Num++;
//
//            break;
//        case 38:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//                g_Auto_Num++;
//            }
//            break;
//        /////路径十结束
//        case 39:
//            /****左取苗开始****/
//
//            chassis[1] = 1;
//
//        /****左取苗结束****/
//        //            g_Auto_Num++;
//        //            Motor_ClearPID();
//            break;
//        /////路径十一开始                 //第七次放苗
//        case 40:
//            Control_Point.Path_BesselPoint_1 = Robot_Pose;
//
//            Control_Point.Path_BesselPoint_2.x = -1161.86f;
//            Control_Point.Path_BesselPoint_2.y = 969.f;
//
//            Control_Point.Path_BesselPoint_3.x = -1312.43f;
//            Control_Point.Path_BesselPoint_3.y = 1303.31f;
//
//            Control_Point.Path_BesselPoint_4.wx = -1150.43f;
//            Control_Point.Path_BesselPoint_4.wy = 1792.53f;
//            Control_Point.Path_BesselPoint_4.yaw = -630.f;
//
//            g_Auto_Num++;
//
//            break;
//        case 41:
//            if (Path_BesselPathInit(&Control_Point, false, 2000, 2500, 100, 5, 40, 70));
//            g_Auto_Num++;
//
//            break;
//        case 42:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//                g_Auto_Num++;
//            }
//            break;
//        /////路径十一结束
//        case 43:
//            /****左放苗1开始****/
//
//            chassis[4] = 1;
//
//        /****左放苗1结束****/
//        //            g_Auto_Num++;
//            break;
//        /////路径十二开始                     //第八次放苗
//        case 44:
//            //            if(Pack_receive.is_start_chassic) {
//            line.startPose = Robot_Pose;
//            line.endPose = gopath12;
//            line.endPose.yaw = optimizeRobRotation(line.endPose.yaw);
//            g_Auto_Num++;
//        //            }
//            break;
//        case 45:
//            if (StraigthLinePathInit(&line, false, 1200, 1200, 100, 0, 25, 20));
//            g_Auto_Num++;
//            break;
//        case 46:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//                g_Auto_Num++;
//            }
//            break;
//        /////路径十二结束
//        case 47:
//            /****左放苗2开始****/
//
//            chassis[5] = 1;
//
//        /****左放苗2结束****/
//        //            g_Auto_Num++;
//            break;
//        /////路径十三开始             //平移到坡前
//        case 48:
//            line.startPose = Robot_Pose;
//            line.endPose = gopath13;
//            line.endPose.yaw = optimizeRobRotation(line.endPose.yaw);
//            g_Auto_Num++;
//            break;
//        case 49:
//            if (StraigthLinePathInit(&line, false, 1500, 2500, 1000, 5, 30, 60));
//            g_Auto_Num++;
//            break;
//        case 50:
//            if (goPath() == FINISH) {
//                //                setPidCalMode(PID_OFF);
//                g_Auto_Num++;
//            }
//            break;
//        case 51:
//            //            setAirCylinder_more(Air_1,Close_Air,5,OUT_3,OUT_4,OUT_5,OUT_6,OUT_7);
//            //            Pack_receive.is_chassic_end=true;
//            g_Auto_Num++;
//
//            break;
//        /////路径十三结束
//        /////路径十四开始             //开始上坡
//        case 52:
//
//            line.startPose = Robot_Pose;
//            line.endPose = gopath14;
//            line.endPose.yaw = optimizeRobRotation(line.endPose.yaw);
//            g_Auto_Num++;
//
//            break;
//        case 53:
//            if (StraigthLinePathInit(&line, false, 2000, 3000, 800, 10, 25, 20));
//            g_Auto_Num++;
//            break;
//        case 54:
//            if (goPath() == FINISH) {
//                setPidCalMode(PID_OFF);
//
//
//                g_Auto_Num++;
//            }
//            break;
//        case 55:
//            Only_Once = 1;
//        //            Hand_Shank_Data.Flag_Start_HandShank = true;
//        //            Hand_Shank_Data.Flag_Start_HandShank = true;
//            if (Only_Once == 1) {
//                Only_Once++;
//                Dijkstra_PathPlanning(28);
//                Flag_Finish1Task = true;
//            }
//        //            Motor_ClearPID();
//            break;
//        default:
//            break;
//    }
//}
//
uint8_t Loop_Num = 0;
uint8_t Knots_Num = 0;
extern bool flag_zero;
extern float Angle;

static bool flag_fixyaw = false;

bool flag_MatchingNode = true;

bool once_togo = false;

uint8_t Current_PathNum = 255;

extern bool TurnLock;
extern bool SpeedLock;

extern float percent;

extern PATH_STATE pathState;

uint8_t current_knot = 255;

void GoGetBallTask(void) {
    static PATH_BesselControlPoint Control_Point;
    static float Last_TargetYaw = 0;
    static Knots_state NextPoint = {0};
    static bool once = false;
    switch (Loop_Num) {
        case 0: ///路径：起点 终点 贝塞尔控制点载入，
        {
            TaskFlag.Flag_UseManualYaw = false;
            if (Flag_ManualStart) {
                ///手动转自动 到达最近的节点
                if (TaskFlag.Flag_BlueZone && !TaskFlag.Flag_RedZone) {
                    NextPoint = Dijkstra_NodeMappingCoordinates_Bule(PathKnots[Knots_Num]);
                } else if (TaskFlag.Flag_RedZone && !TaskFlag.Flag_BlueZone) {
                    NextPoint = Dijkstra_NodeMappingCoordinates_Red(PathKnots[Knots_Num]);
                }
                if (flag_MatchingNode) {
                    Control_Point.Path_BesselPoint_1 = Robot_Pose;

                    Control_Point.Path_BesselPoint_2.x = Robot_Pose.wx;
                    Control_Point.Path_BesselPoint_2.y = (NextPoint.wy - Robot_Pose.wy) * 0.3f + Robot_Pose.wy;

                    Control_Point.Path_BesselPoint_3.x = Robot_Pose.wx;
                    Control_Point.Path_BesselPoint_3.y = (NextPoint.wy - Robot_Pose.wy) * 0.6f + Robot_Pose.wy;

                    Control_Point.Path_BesselPoint_4.wx = Robot_Pose.wx;
                    Control_Point.Path_BesselPoint_4.wy = NextPoint.wy;
                } else {
                    Control_Point.Path_BesselPoint_1 = Robot_Pose;

                    Control_Point.Path_BesselPoint_2.x = (NextPoint.wx - Robot_Pose.wx) * 0.3f + Robot_Pose.wx;
                    Control_Point.Path_BesselPoint_2.y = Robot_Pose.wy;

                    Control_Point.Path_BesselPoint_3.x = (NextPoint.wx - Robot_Pose.wx) * 0.6f + Robot_Pose.wx;
                    Control_Point.Path_BesselPoint_3.y = Robot_Pose.wy;

                    Control_Point.Path_BesselPoint_4.wx = NextPoint.wx;
                    Control_Point.Path_BesselPoint_4.wy = Robot_Pose.wy;

                    Last_TargetYaw = NextPoint.yaw;
                }
            } else {
                ///一般情况，从节点的映射数组中获取去往下一个点路径信息
                if (TaskFlag.Flag_BlueZone && !TaskFlag.Flag_RedZone) {
                    NextPoint = Dijkstra_NodeMappingCoordinates_Bule(PathKnots[Knots_Num]);
                    // printf("Bule\r\n");
                } else if (TaskFlag.Flag_RedZone && !TaskFlag.Flag_BlueZone) {
                    NextPoint = Dijkstra_NodeMappingCoordinates_Red(PathKnots[Knots_Num]);
                    // printf("Red\r\n");
                }
                if (PathKnots[Knots_Num] < 30 || PathKnots[Knots_Num] == 34) {
                    ///使用贝塞尔曲线在二区跑直线，数学问题我不想解释
                    Control_Point.Path_BesselPoint_1 = Robot_Pose;

                    Control_Point.Path_BesselPoint_2.x = (NextPoint.wx - Robot_Pose.wx) * 0.3f + Robot_Pose.wx;
                    Control_Point.Path_BesselPoint_2.y = (NextPoint.wy - Robot_Pose.wy) * 0.3f + Robot_Pose.wy;

                    Control_Point.Path_BesselPoint_3.x = (NextPoint.wx - Robot_Pose.wx) * 0.6f + Robot_Pose.wx;
                    Control_Point.Path_BesselPoint_3.y = (NextPoint.wy - Robot_Pose.wy) * 0.6f + Robot_Pose.wy;

                    Control_Point.Path_BesselPoint_4.wx = NextPoint.wx;
                    Control_Point.Path_BesselPoint_4.wy = NextPoint.wy;
                    if (PathKnots[Knots_Num] >= 17) {
                        Last_TargetYaw = NextPoint.yaw;
                    } else {
                        Last_TargetYaw = 0.0f;
                    }
                } else {
                    ///一区跑曲线
                    Control_Point.Path_BesselPoint_1 = Robot_Pose;

                    Control_Point.Path_BesselPoint_2.x = NextPoint.BesselControlPoint_1.x;
                    Control_Point.Path_BesselPoint_2.y = NextPoint.BesselControlPoint_1.y;

                    Control_Point.Path_BesselPoint_3.x = NextPoint.BesselControlPoint_2.x;
                    Control_Point.Path_BesselPoint_3.y = NextPoint.BesselControlPoint_2.y;

                    Control_Point.Path_BesselPoint_4.wx = NextPoint.wx;
                    Control_Point.Path_BesselPoint_4.wy = NextPoint.wy;
                }
            }
            if (PathKnots[Knots_Num] == 26 || PathKnots[Knots_Num] == 24) {
                Last_TargetYaw = NextPoint.yaw;
            }
            if (PathKnots[Knots_Num] == 19) {
                Last_TargetYaw = NextPoint.yaw;
            }
            // if (Dijkstra_CountKnots == 1 && PathKnots[Knots_Num] >= 11 && PathKnots[Knots_Num] <= 17) {
            //     ///调整发射角度
            //     Last_TargetYaw = NextPoint.yaw;
            //     setPidCalMode(PID_YAW_ONLY);
            //     Pose shootPose = {0};
            //     shootPose.yaw = NextPoint.yaw;
            //     shootPose.wx = Robot_Pose.wx;
            //     shootPose.wy = Robot_Pose.wy;
            //     Loop_Num = 3;
            //     setPidTargetPose(&shootPose, PID_YAW_ONLY);
            // }
            Control_Point.Path_BesselPoint_4.yaw = Last_TargetYaw;
            // printf("Knots_Num:%d,NextPoint_x:%f,NextPoint_y:%f,NextPoint_yaw:%f\r\n", PathKnots[Knots_Num],
                   // NextPoint.wx, NextPoint.wy, Control_Point.Path_BesselPoint_4.yaw);
            Loop_Num++;
            TurnLock = true;
            SpeedLock = true;
            current_knot = PathKnots[Knots_Num];
            break;
        }
        case 1:
            ///生成路径点
            if (Path_BesselPathInit(&Control_Point, false, NextPoint.statspeed, NextPoint.maxspeed,
                                    NextPoint.endspeed,
                                    NextPoint.acceleratePercent, NextPoint.descentPercent,
                                    NextPoint.yawAdjustPercent)) {
                Loop_Num++;
            }
            break;

        case 2:
            // printf("X:%f\tY:%f\tP:%f\tS:%d\r\n",Robot_Pose.wx,Robot_Pose.wy,percent,pathState);
            // printf("%d\r\n", PathKnots[Knots_Num]);
            // if (PathKnots[Knots_Num] == 20) {
                ///路径截断，便于重新标定坐标点
                // if(percent >= 80) {
                // setPidCalMode(PID_OFF);
                // setPathVel(0,0);
                // }
                // if (goPath() == FINISH) {
                    // DJI_SetTargetParam(&M2006_LF, 0 * 1.006712757457386364f, 0);
                    // DJI_SetTargetParam(&M2006_LR, 0 * 1.0068736683238636363636363636364f, 0);
                    // DJI_SetTargetParam(&M2006_RR, 0 * 1.0068736683238636363636363636364f, 0);
                    // Current_PathNum = PathKnots[Dijkstra_CountKnots - 1];
                    // setPidCalMode(PID_OFF);
                    // setPathVel(0,0);
                    // if (TaskFlag.Flag_PathContinue) {
                    // Loop_Num++;
                    // printf("????\r\n");
                    // TaskFlag.Flag_PathContinue = false;
                // }

                // printf("gocn:%d\tpathnum:%d\r\n", gocnt, PathKnots[Knots_Num]);
                // once_togo = false;
                // Loop_Num++;
                // }
            // } else {
                // if ((PathKnots[Knots_Num] >= 0) && (PathKnots[Knots_Num] <= 11)) {
                    // once_togo = false;
                    // Loop_Num++;
                    // setPidCalMode(PID_YAW_ONLY);
                // }
                if (goPath() == FINISH) {
                    ///正常是用这个
                    setPidCalMode(PID_YAW_ONLY);
                    setPathVel(0,0);
                    // goPath();
                    once_togo = false;
                    Loop_Num++;
                }
            // }
            break;
        default:
            if (Flag_ManualStart) {
                ///手动转自动，到达临近节点需要先趋近X轴坐标，再到Y轴坐标，所以跑两次
                if (flag_MatchingNode) {
                    flag_MatchingNode = false;
                    Loop_Num = 0;
                } else {
                    if (!once_togo) {
                        ///获取一下当前节点的任务，感觉没有必要
                        // PathFeedback_to_Master(ManualToAutoMation_FindNearNode(Robot_Pose));
                        Current_PathNum = ManualToAutoMation_FindNearNode(Robot_Pose);
                        Dijkstra_FirstPathPlanning(ManualToAutoMation_FindNearNode(Robot_Pose));
                    } else {
                        Loop_Num = 0;
                        once_togo = false;
                        Flag_ManualStart = false;
                    }
                }
            } else {
                if (Knots_Num + 1 < Dijkstra_CountKnots) {
                    ///看一下当前跑过的节点数量有没有到达目标节点
                    // printf("%d\r\n",PathKnots[Knots_Num]);
                    if (PathKnots[Knots_Num] > 20 && PathKnots[Knots_Num] != 28 && PathKnots[Knots_Num] != 25 &&
                        PathKnots[Knots_Num] != 22 && PathKnots[Knots_Num] != 34) {
                        ///有任务的节点向上层反馈，等待继续指令
                        if (!once_togo) {
                            if (!once) {
                                // PathFeedback_to_Master(PathKnots[Knots_Num]);
                                Current_PathNum = PathKnots[Knots_Num];
                                // printf("%d\r\n", PathKnots[Knots_Num]);
                                once = true;
                            }
                        } else {
                            Loop_Num = 0;
                            Knots_Num++;
                            once_togo = false;
                            once = false;
                        }
                    } else {
                        ///没有任务直接跑
                        // HAL_Delay(1000);
                        Loop_Num = 0;
                        Knots_Num++;
                    }
                } else {
                    ///此次节点路径结束了，算一下下一个节点去哪
                    if (PathKnots[Knots_Num] >= 12 && PathKnots[Knots_Num] <= 17) {
                        TaskFlag.Flag_UseManualYaw = true;
                    }
                    if (PathKnots[Knots_Num] == 18) {
                        TaskFlag.Flag_GoTO5 = true;
                    }
                    //else {
                    //     if (!once_togo) {
                    //         if (!once) {
                    //             // PathFeedback_to_Master(PathKnots[Dijkstra_CountKnots - 1]);
                    //             Current_PathNum = PathKnots[Dijkstra_CountKnots - 1];
                    //             // printf("?%d\r\n", PathKnots[Dijkstra_CountKnots - 1]);
                    //             once = true;
                    //         }
                    //     } else {
                    //         Dijkstra_DijkstraCalculate();
                    //         once_togo = false;
                    //         once = false;
                    //     }
                    // }
                }
            }
            break;
    }
    // printf("%d,%d,%d\n", Loop_Num, Knots_Num, Dijkstra_CountKnots);
}
